from djitellopy import Tello
import cv2

tello = Tello()
try:
    tello.connect()
    tello.streamon()
    # tello.takeoff()

    def handle_airflow():
        threshold = 10
        correction_factor = 1
        roll = tello.get_roll()
        yaw = tello.get_yaw()
        pitch = tello.get_pitch()
        if abs(roll) > threshold or abs(pitch) > threshold:
            print("正在调整！")
            tello.send_rc_control(int(roll * correction_factor), int(pitch * correction_factor), 0, int(yaw * correction_factor))

    while True:
        handle_airflow()
        img = tello.get_frame_read().frame
        rgb_frame = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
        cv2.imshow('Video Stream', rgb_frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    # tello.land()
    tello.streamoff()
    cv2.destroyAllWindows()
except Exception as e:
    print(f"发生错误: {e}")
    tello.streamoff()
    cv2.destroyAllWindows()